// --------------------------------------------------------------------------------------------------------------------
// <copyright file="SinglePicknPlaceFSM.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------

namespace Test.Robotics.Manipulation
{
    using System;
    using System.Collections.Generic;
    using System.Runtime.Serialization;
    using Microsoft.Robotics.FiniteStateMachine;
    using Microsoft.Robotics.Manipulation.PicknPlace;
    using Microsoft.Robotics.Manipulation.Runtime;
    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.Runtime;
    using Microsoft.Robotics.Tracing;

    /// <summary>
    /// FSM for a single pick and place action
    /// </summary>
    public class SinglePicknPlaceFSM : Machine<SinglePicknPlaceFSMContext, SinglePicknPlaceFSMContext, SinglePicknPlaceFSMContext>
    {
        /// <summary>
        /// Initializes a new instance of the <see cref="SinglePicknPlaceFSM" /> class
        /// </summary>
        /// <param name="context">Context for state machine</param>
        /// <param name="pickMovePost">Call back to post single pick and move requests</param>
        /// <param name="placeMovePost">Call back to post single place and move requests</param>
        /// <param name="overallInProgressStatus">Overall in progress status to report</param>
        /// <param name="overallCompletionStatus">Overall completion status to report </param>
        public SinglePicknPlaceFSM(
            SinglePicknPlaceFSMContext context,
            Action<PickAndMove> pickMovePost,
            Action<PlaceAndMove> placeMovePost,
            PicknPlaceStatusEnum overallInProgressStatus,
            PicknPlaceStatusEnum overallCompletionStatus)
        {
            this.Context = context;
            int iterCount = 0;

            State<SinglePicknPlaceFSMContext> start = new State<SinglePicknPlaceFSMContext>(
                 "Start state",
                 c =>
                 {
                     iterCount = 0;
                     return c;
                 });

            State<SinglePicknPlaceFSMContext> sendPick = new State<SinglePicknPlaceFSMContext>(
                 "Sending single pick request",
                 c =>
                 {
                     if (iterCount < this.Context.PickList.Length)
                     {
                         pickMovePost(this.Context.PickList[iterCount]);
                     }

                     return c;
                 });

            State<SinglePicknPlaceFSMContext> waitForPick = new State<SinglePicknPlaceFSMContext>("Waiting for single pick to complete", c => { c.OverallContextStatus = overallInProgressStatus; return c; });

            State<SinglePicknPlaceFSMContext> sendPlace = new State<SinglePicknPlaceFSMContext>(
                 "Sending single place request",
                 c =>
                 {
                     if (iterCount < this.Context.PlaceList.Length)
                     {
                         placeMovePost(this.Context.PlaceList[iterCount]);
                     }

                     return c;
                 });

            State<SinglePicknPlaceFSMContext> waitForPlace = new State<SinglePicknPlaceFSMContext>("Waiting for single place to complete", c => { return c; });
            State<SinglePicknPlaceFSMContext> singleOperationComplete = new State<SinglePicknPlaceFSMContext>(
                "Single pick place Complete", 
                c => 
                {
                    ++iterCount;
                    if (iterCount >= this.Context.PickList.Length &&
                        iterCount >= this.Context.PlaceList.Length)
                    {
                        this.Context.OverallContextStatus = overallCompletionStatus;
                    }

                    return c; 
                });

            start.AddTransition(t => true, sendPick);

            sendPick.AddTransition(t => t.SingleOperationStatus == PicknPlaceStatusEnum.PlaceComplete || t.SingleOperationStatus == PicknPlaceStatusEnum.NotStarted, sendPick);
            sendPick.AddTransition(t => t.SingleOperationStatus == PicknPlaceStatusEnum.PickInProgress, waitForPick);
            sendPick.AddTransition(t => t.SingleOperationStatus == PicknPlaceStatusEnum.PickComplete, sendPlace);
            waitForPick.AddTransition(t => t.SingleOperationStatus == PicknPlaceStatusEnum.PickComplete, sendPlace);
            sendPlace.AddTransition(t => t.SingleOperationStatus == PicknPlaceStatusEnum.PlaceInProgress, waitForPlace);
            sendPlace.AddTransition(t => t.SingleOperationStatus == PicknPlaceStatusEnum.PickComplete || t.SingleOperationStatus == PicknPlaceStatusEnum.NotStarted, sendPlace);
            waitForPlace.AddTransition(t => t.SingleOperationStatus == PicknPlaceStatusEnum.PickComplete, sendPlace);
            waitForPlace.AddTransition(t => t.SingleOperationStatus == PicknPlaceStatusEnum.PlaceComplete, singleOperationComplete);
            singleOperationComplete.AddTransition(t => iterCount < this.Context.PickList.Length && iterCount < this.Context.PlaceList.Length, sendPick);

            this.Initialize(this.Context, start);
        }

        /// <summary>
        /// Step the FSM
        /// </summary>
        public void Step()
        {
            this.Update(this.Context);
        }
    }
}
